from point import Point
from mechanism import Mechanism

import numpy as np
import sympy as sp  # 实现求导
from sympy.abc import t # 引入时间t的符号，等价于t = sp.symbols('t')


class Rob(Mechanism):
  """
  index: 杆件唯一编号，自动分配
  length: 杆长
  basePoint,endPoint:基点与终点
  
  phi,w,wa:角度、角速度、角加速度
  """
  def __init__(self,length,basePoint:Point,symbol_phi,endPoint = None):
    """
    Args:
      length:杆长
      basePoint:基点
      phi:倾角的符号时间函数,单位s
    """
    self._init_index()
    self._init_angle_funcs(symbol_phi)

    self.length = length
    self.basePoint = basePoint

    if(endPoint):
      self.endPoint = endPoint
    else:
      self.endPoint = Point(rx=self.basePoint.symbol_rx+self.length*sp.cos(self.symbol_phi),
                          ry=self.basePoint.symbol_ry+self.length*sp.sin(self.symbol_phi))
    
    self.update(0)


  def _init_index(self):
    """初始化杆件编号"""
    if(hasattr(Rob,'rob_num')):Rob.rob_num+=1
    else:Rob.rob_num = 1
    self.index = Rob.rob_num-1

  
  def _init_angle_funcs(self,symbol_phi):
    """
    初始化角参量
    """
    # 符号函数
    self.symbol_phi = symbol_phi
    # self.symbol_w= symbol_phi.diff(t)
    # self.symbol_wa = symbol_phi.diff(t,2)

    # 数值函数
    self.f_phi = sp.lambdify(t, self.symbol_phi, 'numpy')  # 转换为numpy函数
    if(isinstance(symbol_phi,int)):
      self.symbol_w = 0
      self.symbol_wa = 0
      self.f_w = lambda x:0
      self.f_wa = lambda x:0
    else:
      self.f_w = sp.lambdify(t, self.symbol_phi.diff(t), 'numpy')
      self.f_wa = sp.lambdify(t, self.symbol_phi.diff(t,2), 'numpy')
    

  def update(self,t_s):
    """
    根据时间更新参数
    """
    # 线参量
    self.basePoint.update(t_s)
    self.endPoint.update(t_s)

    # 角参量
    self.phi = self.f_phi(t_s)
    self.w = self.f_w(t_s)
    self.wa = self.f_wa(t_s)
    

  def get_robs(self):
    return [self]

  

if __name__ == '__main__':
  r = Rob(300,Point((0,0)),sp.cos(t))
  
  